#!/usr/bin/env python3

import rospy
from gazebo_msgs.srv import GetModelState, GetModelStateRequest
import numpy as np 


class model_state:
    def __init__(self,model_name = 'aliengo_gazebo',command="/gazebo/get_model_state") -> None:
        self.model_name = model_name
        self.command = command
        # rospy.init_node('get_model_state_client', anonymous=True)
        

    def get_state(self):
        # 创建一个ServiceProxy来调用GetModelState服务
        rospy.wait_for_service(self.command)
        try:
            get_model_state_service = rospy.ServiceProxy(self.command, GetModelState)
            
            # 创建一个请求对象并设置model_name
            request = GetModelStateRequest()
            request.model_name = self.model_name
            
            # 调用服务并获取响应
            response = get_model_state_service(request)
            
            if response.success:
                # print(f"Model State for {self.model_name}:")
                # print(f"Position: ({response.pose.position.x}, {response.pose.position.y}, {response.pose.position.z})")
                # print(f"Orientation (Quaternions): ({response.pose.orientation.x}, {response.pose.orientation.y}, {response.pose.orientation.z}, {response.pose.orientation.w})")
                # print(f"Linear Velocity: ({response.twist.linear.x}, {response.twist.linear.y}, {response.twist.linear.z})")
                # print(f"Angular Velocity: ({response.twist.angular.x}, {response.twist.angular.y}, {response.twist.angular.z})")
                model_state_list = [
                        response.pose.position.x,
                        response.pose.position.y,
                        response.pose.position.z,
                        response.pose.orientation.x,
                        response.pose.orientation.y,
                        response.pose.orientation.z,
                        response.pose.orientation.w,
                        response.twist.linear.x,
                        response.twist.linear.y,
                        response.twist.linear.z,
                        response.twist.angular.x,
                        response.twist.angular.y,
                        response.twist.angular.z
                    ]
                
                return np.array(model_state_list)
            else:
                return np.zeros(shape=13,dtype=np.float32)
        except rospy.ServiceException as e:
            print("Service call failed: %s"%e)
            return np.zeros(shape=13,dtype=np.float32)




if __name__ == "__main__":
    geter=model_state(model_name = 'aliengo_gazebo',command="/gazebo/get_model_state")
    geter.get_state()

    pass